

#ifndef ROB_VECTOR_H
#define ROB_VECTOR_H

#include <cmath>

#include "matrix.h"



template<size_t n, typename T=Scalar>
class Vector : public Matrix<1,n,T>
{
  public:
    Vector(T data_[n]) { for(size_t i=0;i<n;++i) this->data[i]=data_[i]; };
    Vector() {};

    Scalar Length() const { return sqrt((*this)*(*this)); }

    // Accessors (by reference)
    T&       Element(size_t i)       { return Matrix<1,n,T>::Element(0,i); }
    const T& Element(size_t i) const { return Matrix<1,n,T>::Element(0,i); }


    Vector<n,T> operator+ (const Vector<n,T>&) const;
    Vector<n,T> operator- (const Vector<n,T>&) const;
    Vector<n,T> operator* (Scalar x) const;
    T operator* (const Vector<n,T>& rhs) const { return DotProduct(*this,rhs); };

};

template<size_t n, typename T>
Vector<n,T> Vector<n,T>::operator+ (const Vector<n,T>& rhs) const
{
    Vector<n,T> ret(*this);
    for(size_t i=0;i<n;++i)
      ret.Element(i)+=rhs.Element(i);
    return ret;
};
// Matrix Subtraction
template<size_t n, typename T>
Vector<n,T> Vector<n,T>::operator- (const Vector<n,T>& rhs) const
{
    Vector<n,T> ret(*this);
    for(size_t i=0;i<n;++i)
      ret.Element(i)-=rhs.Element(i);
    return ret;
};
// Scalar Multiplication
template<size_t n, typename T>
Vector<n,T> Vector<n,T>::operator* (Scalar x) const
{
    Vector<n,T> ret(*this);
    for(size_t i=0;i<n;++i)
      ret.Element(i)*=x;
    return ret;
}


// GLOBAL FUNCTIONS
template<size_t n, typename T>
Scalar DotProduct(const Vector<n,T>& lhs, const Vector<n,T>& rhs, Matrix<n,n,T> A=Identity<n,T>())
{
    return ( (Matrix<1,n,T>)lhs * A * rhs.Transpose() ).Value();
}

template<size_t n, typename T>
Vector<n,T> CrossProduct(const Vector<n,T>& lhs, const Vector<n,T>& rhs)
{
    // TODO use determinant of underlying Matrix class
}

template<size_t n, typename T>
Scalar Angle(const Vector<n,T>& lhs, const Vector<n,T>& rhs)
{
    return acos((lhs*rhs)/(lhs.Length()*rhs.Length()));
}

template<size_t n, typename T>
Scalar Distance(const Vector<n,T>& lhs, const Vector<n,T>& rhs)
{
    return (lhs-rhs).Length();
}

template<size_t n, typename T>
bool IsPerpendicular(const Vector<n,T>& lhs, const Vector<n,T>& rhs)
{
    return lhs*rhs == Scalar(0);
}

#endif // ROB_VECTOR_H

